#pragma once
#include <vector>
#include <string>
#include <queue>
#include <mutex>
#include <thread>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "io_macro.h"
#include "agv_srvs/srv/output_interface.hpp"
#include "agv_msgs/msg/bat_info.hpp"
#include "agv_msgs/msg/connection_monitor.hpp"
#include "agv_msgs/msg/pose_with_confidence.hpp"
#include "agv_msgs/msg/agv_event_status.hpp"

using namespace std::chrono_literals;

// 参数： first:io索引 第几个io点 second:控制输出的电平
typedef std::pair<int, bool> OUTPUT_IO_TYPE;
typedef std::vector<OUTPUT_IO_TYPE> IO_GROUP;

class COutPutIo {
public:
    explicit COutPutIo(std::shared_ptr<rclcpp::Node>& node):
        node_(node),
        m_client(nullptr),
        m_bValidEachtime(false),
        ctrl(-1)
    {     
    }

    bool init(std::shared_ptr<rclcpp::Client<agv_srvs::srv::OutputInterface>> client) {
        m_client = client;
        return true;
    }

    bool set(bool level) {
        bool ret = false;
        if (ctrl < 0) {
            DEBUG_STREAM_OUT("set io out, ctrlIndex is not exist");
            return ret;
        }

        IO_GROUP ioGroup;
        ioGroup.push_back({ctrl, level});
        ret = outputIo(ioGroup);
        return ret;
    }

    void setValidEachTime(bool valid) {
        m_bValidEachtime = valid;
        DEBUG_OUT("setValidEachTime:valid=%d", valid);
    }

    int ctrl;

private:
    std::shared_ptr<rclcpp::Node> node_;
    IO_GROUP m_outIoRecord;
    std::shared_ptr<rclcpp::Client<agv_srvs::srv::OutputInterface>> m_client;
    bool m_bValidEachtime;

    bool outputIo(IO_GROUP outIoGroup) {
        if (!m_client->wait_for_service(1s)) {
            DEBUG_STREAM_OUT("Service not available");
            return false;
        }

        auto request = std::make_shared<agv_srvs::srv::OutputInterface::Request>();
        for (auto& outIo : outIoGroup) {
            agv_msgs::msg::OutputPort port;
            port.index = outIo.first;
            port.level = outIo.second;
            request->index_level.push_back(port);
        }

        auto future = m_client->async_send_request(request);
        if (future.wait_for(1s) == std::future_status::ready) {
            return future.get()->state;
        }
        return false;
    }
};

class OutPutIoControl {
public:
    OutPutIoControl(std::shared_ptr<rclcpp::Node>& node): 
        node_(node),
        netConnectLight(node),  
        lowBatteryLight(node),
        locationLight(node),
        emgLight(node) 
    {
        netConnectLight.ctrl = node_->get_parameter_or("output_device/netConnectLight", -1);
        lowBatteryLight.ctrl = node_->get_parameter_or("output_device/lowBatteryLight", -1);
        locationLight.ctrl = node_->get_parameter_or("output_device/locationLight", -1);
        emgLight.ctrl = node_->get_parameter_or("output_device/emgLight", -1);

        m_client = node_->create_client<agv_srvs::srv::OutputInterface>("/outputinterface");
        
        netConnectLight.init(m_client);
        lowBatteryLight.init(m_client);
        locationLight.init(m_client);
        emgLight.init(m_client);
    }

    COutPutIo netConnectLight;
    COutPutIo lowBatteryLight;
    COutPutIo locationLight;
    COutPutIo emgLight;

private:
    std::shared_ptr<rclcpp::Node> node_;
    std::shared_ptr<rclcpp::Client<agv_srvs::srv::OutputInterface>> m_client;
};

struct MsgToIo {
    agv_msgs::msg::ConnectionMonitor netMsg;
    agv_msgs::msg::PoseWithConfidence poseMsg;
    agv_msgs::msg::BatInfo batMsg;
    agv_msgs::msg::AGVEventStatus eventMsg;
};

class SpecialIo {
public:
    explicit SpecialIo(std::shared_ptr<rclcpp::Node>& node);
    ~SpecialIo();
    
private:
    std::shared_ptr<rclcpp::Node> node_;
    std::shared_ptr<OutPutIoControl> m_ioControl;
    std::timed_mutex m_quit_mutex;
    std::thread m_thread;
    MsgToIo m_msg_to_io;
    bool m_init_ok;

    rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr m_pose_sub;
    rclcpp::Subscription<agv_msgs::msg::BatInfo>::SharedPtr m_battery_sub;
    rclcpp::Subscription<agv_msgs::msg::ConnectionMonitor>::SharedPtr m_network_connection_sub;
    rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr m_event_sub;
    
    void cycle();
    static void thread_fun(SpecialIo *pThis);
   
    void batteryCB(const agv_msgs::msg::BatInfo::SharedPtr& msg) {
        m_msg_to_io.batMsg = *msg;
    }

    void poseCB(const agv_msgs::msg::PoseWithConfidence::SharedPtr& msg) {
        m_msg_to_io.poseMsg = *msg;
    }

    void networkCB(const agv_msgs::msg::ConnectionMonitor::SharedPtr& msg) {
        m_msg_to_io.netMsg = *msg;
    }

    void eventCB(const agv_msgs::msg::AGVEventStatus::SharedPtr& msg) {
        m_msg_to_io.eventMsg = *msg;
    }
};